﻿/*
 * 主要功能是根据机器人协议提供控制机器人的命令字段
 * 
 * 关注下是否机器人会自动返回特殊指定，这个需要时刻接受并且监听处理
 */
package com.robotcontroller.buaa.client;

public class RobotControlCommand
{
	/* 机器人的设备ID
	 * ID0，电源
	 * ID1，机器人底盘
	 * ID2，手机夹持机构
	 * ID3，夏普红外测距传感器
	 * ID4，红外蔽障传感器
	 * ID5，巡线传感器
	 * ID6，三色灯
	 * ID7，码盘
	 * ID8，RFID定位
	 * ID9，环境建图，即超声波测距
	 */
	private byte[] ID = {0x01, 0x02 , 0x03 , 0x04 , 0x05 , 0x06 , 0x07};
	
	//private static final byte maxcmdlen = 0x20;  //命令的最大长度
	//public byte[] command = new byte[maxcmdlen]; //欲发送给机器人的命令
	//public byte commandlength;                          //命令的长度
	
	
	/*
	 * 机器人返回的协议信息解析出来的设备ID和ID的值
	 */
	public static class  RobotDevIDInfor
	{
		public static byte ID = (byte) 0xFF;
		public  static int IDInforLen = 50;   //50字节应该足够大
		public  static byte[] IDInfor = new byte[IDInforLen];
	}
	
	public RobotControlCommand() 
	{
		
	}
	
	
	/*
	 * 向机器人发送的写命令，控制机器人的行为，机器人不返回信息
	 * 参数id，设备ID
	 * 参数argv，控制设备的具体参数，长度不定
	 * 线程需要互斥的调用该函数
	 * 虽然每次都new，但是jvm会管理动态内存，不像c++那样需要用户管理
	 */
   public synchronized byte[] W_CMDtoRobot(byte id  , byte[] argv)
   {
	   byte[] cmd;      // = command; //欲发送给机器人的命令
	   byte cmdlen;     //整条命令的长度
	   byte framelen;   //帧长度
	   
	   int argvlen;
	   if(null == argv)
	   {
		   argvlen = 0;
	   }
	   else
	   {
		   argvlen = argv.length;
	   }
	   
	   //帧长度 =ID长度+命令长度+命令参数长度+帧尾长度
	   framelen = (byte) (1 + 1 + argvlen + 1);   
	   
	   //命令长度 = 帧头 + 帧长度字段 + 帧长度
	   cmdlen = (byte) (2 + 1 + framelen);
	   
	   /*
	   if(cmdlen > maxcmdlen)
	   {
		   return null;  //命令长度超出范围，返回空，这个是由软件设计人员所写，
		                 //应该不需要这个判断
	   }
	   */
	   
	   
	   // commandlength = cmdlen;
	   cmd = new byte[cmdlen];
	   
	   //帧头AAFF，两个字节
	   cmd[0] = (byte) 0xAA;
	   cmd[1] = (byte) 0xFF;
	   
	   //帧长度，一个字节
	   cmd[2] = (byte) framelen;
	   
	   //设备ID
	   cmd[3] = id;
	   
	   //命令
	   cmd[4] = 0x01;
	   
	   //命令的参数
	   //cmd[5]——cmd[5+argv.length]
	   //System.arraycopy(cmd, 5, argv , 0 , argv.length);//不要写反了
	   if(argv != null)
	   {
		   System.arraycopy(argv, 0, cmd , 5 , argv.length);
	   }
	 
	   
	   //帧尾
	   cmd[5+argvlen] = (byte) 0xFF;
	   
	   
	   return cmd;
   }
   
   /*
    * 向机器人发送读的命令，读取机器人或者机器人传感器的状态，机器人返回信息
    */
   public synchronized byte[] R_CMDtoRobot(byte id  , byte[] argv)
   {
	   byte[] cmd;      // = command; //欲发送给机器人的命令
	   byte cmdlen;     //整条命令的长度
	   byte framelen;   //帧长度
	   
	   int argvlen;
	   if(null == argv)
	   {
		   argvlen = 0;
	   }
	   else
	   {
		   argvlen = argv.length;
	   }
	   //帧长度 =ID长度+命令长度+命令参数长度+帧尾长度
	   framelen = (byte) (1 + 1 + argvlen + 1);   
	   
	   //命令长度 = 帧头 + 帧长度字段 + 帧长度
	   cmdlen = (byte) (2 + 1 + framelen);
	   
	   // commandlength = cmdlen;
	   cmd = new byte[cmdlen];
	   
	   //帧头AAFF，两个字节
	   cmd[0] = (byte) 0xAA;
	   cmd[1] = (byte) 0xFF;
	   
	   //帧长度，一个字节
	   cmd[2] = (byte) framelen;
	   
	   //设备ID
	   cmd[3] = id;
	   
	   //命令
	   cmd[4] = 0x00;
	   
	   if(argv != null)
	   {
		   System.arraycopy(argv, 0, cmd , 5 , argv.length);
	   }

	  	   
	   //帧尾
	   cmd[5+argvlen] = (byte) 0xFF;   
	   
	   return cmd;
   }
   
   
  /*
   * 解析机器人 的命令
   * 设备ID，设备返回的值！
   */
   public  RobotDevIDInfor ParseRobotCMD(byte[] buffer , int len)
   {
	
	   
	   //解析buffer的内容，根据协议解析
	   /*
	    * BB FF    04       01     00  00    FF
        * 帧头          帧长度       设备ID      参数              帧尾
        *                
	    */
	   
	   
	   byte[] cmd = new byte[len];
	   System.arraycopy(buffer, 0, cmd , 0 , len);
	   int cmdlen = len ; //整条命令的长度
	   int framelen ;     //帧长度
	   byte ID;           //传感器设别ID
	   byte IDInforLen;   //ID的值得长度，1个字节，或2个字节，或更多IDInforLen = 帧长度-2
	   
	   /*
	   if(0xBB != cmd[0] || 0xFF != cmd[1])  //0,1字节为帧头
	   {
		   return -1;  //帧头信息不对
	   }
	   */
	   
	   framelen = cmd[2];
	   IDInforLen = (byte) (framelen - 2);
	   ID = cmd[3];
	   
	   
	   /*
	    * cmd[4]到cmd[4+IDInforLne]为ID的值
	    * 根据协议提出各自的值，有的是1个字节，有的2个字节，有的是4个字节，
	    * 根据协议转换为整型的值
	    */
	   switch(ID)
	   {
		   case 0:  //巡线结束的信息
		   {
			   break;
		   }		   
		   case 2:   //手机夹持机构的信息，即角度，1个字节
		   {
			   break;
		   }
		   case 3:  //红外短距测距，距离，2个字节，先取第二个字节测试看看
		   {
			   break;
		   }
		   case 8:   //RFID值，4个字节
		   {
			   break;
		   }
		   case 9:   //环境建图，
		   {
			   break;
		   }
		   
	   }
	   
	   
	   RobotDevIDInfor rbtinfor = new RobotDevIDInfor();
	   
	   rbtinfor.IDInforLen = IDInforLen;
	   System.arraycopy(cmd, 4 , rbtinfor.IDInfor , 0, IDInforLen);
	   rbtinfor.ID = ID;
	   
	   
	   return rbtinfor;
	   
	   //return ID;

   }
   
	   /*
	    * 将字节转换成无符号的整形数字
	    * 例如0xf9，不转换为-7，转换后位249
	    */
	   public int ByteToUnint(byte byteval)
	   {
	   	int intval=0;
	   	for(int i=0; i<=7 ; i++)
	   	{
	   		if( (byteval & (byte)Math.pow(2, i)) != 0)
	   		{
	   			intval = (int) (intval +  Math.pow(2, i));
	   		}
	   	}
	   	return intval;
	   }
   
   
}